/*
* Change Logs:
* Date            Author          Notes
* 2023-09-05      ChuShicheng     first version
*/

#include "example_task.h"
#include "pid.h"
#include "Motor.h"

float target_speed_L;
float target_speed_R;
float vw;
float front_speed;
float mv_alpha;


extern int8_t Car_Do_With_Ball_Mode ;

int8_t Car_Stop;

static struct chassis_controller_t{
    pid_obj_t *speed_pid_L;
    pid_obj_t *speed_pid_R;
    pid_obj_t *alpha_pid;
}chassis_controller;

motor_measure_t motor_L_measure
={
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    &htim4,
    0,

  };

motor_measure_t motor_R_measure
={
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    &htim5,
    0,
  };

float ChaSuJiaoZheng = 0.7f;

    volatile int16_t set = 0;
static int16_t chassis_control(motor_measure_t measure ,pid_obj_t* pid ,float target_speed){
    set = pid_calculate(pid, measure.Speed, target_speed);
    // if(measure.Speed > 0)
    //     set+=8;
    // if(measure.Speed < 0)
    //     set-=8;
    return set;
}
static int16_t chassis_control_alpha(float measure_alpha ,pid_obj_t* pid ,float target_alpha){
    set = -pid_calculate(pid, measure_alpha, target_alpha);
    return set;
}



 void example_init()
{
    pid_config_t chassis_speed_config_L = {
            .Kp = 10, // 4.5
            .Ki = 0,  // 0
            .Kd = 0,  // 0
            .IntegralLimit = 3000,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .MaxOut = 12000,
    };
    chassis_controller.speed_pid_L = pid_register(&chassis_speed_config_L);
        pid_config_t chassis_speed_config_R = {
            .Kp = 10, // 4.5
            .Ki = 0,  // 0
            .Kd = 0,  // 0
            .IntegralLimit = 3000,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .MaxOut = 12000,
    };
    chassis_controller.speed_pid_R = pid_register(&chassis_speed_config_R);

    pid_config_t chassis_alpha_config = {
        .Kp = 0.5f, // 4.5
        .Ki = 2.0f /*10.0f*/,  // 0  2.0未试，10.0不行，还不确定是不是积分项的问题
        .Kd = 0,  // 0
        .IntegralLimit = 20,
        .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
        .MaxOut = 50,
    };
    chassis_alpha_config.CoefA = 20;
    chassis_alpha_config.CoefB = 0;
    chassis_controller.alpha_pid = pid_register(&chassis_alpha_config);

}

void example_thread_entry()
{
    static float example_dt;
    static float example_start;
    // example_init();
    // LOG_I("Example Task Start");
    static uint32_t example_step;
    example_step ++;
    example_step %= 1;  //轮询执行，根据总CPU使用情况查看motor_measure.error_time修改，目前测试10-10ms执行一次


    if(example_step == 0)
    {


        vw = -mv_alpha*0.5; //和球的夹角，期望为0,球在左mv_alpha>0,error<0,pid_out<0,取反

        //目前PID效果不行,先不用
        // if(Car_Do_With_Ball_Mode == 2)  //回家模式使用该PID
            // vw = chassis_control_alpha(0,chassis_controller.alpha_pid,mv_alpha);
        target_speed_L = front_speed*13 - vw;
        target_speed_R = front_speed*13 + vw;
        // if(Car_Stop == 1)
        // {
            // target_speed_L = 0;
            // target_speed_R = 0;
        // }
        target_speed_L = target_speed_L * ChaSuJiaoZheng;  //实际控制的是右边的轮子
         Motor_Control(&htim1,TIM_CHANNEL_2,target_speed_R);
        Motor_Control(&htim1,TIM_CHANNEL_3,target_speed_L);
    }
        /* 用于调试监测线程调度使用 */
        // example_dt = dwt_get_time_ms() - example_start;
        // if (example_dt > 1)
            // LOG_E("Example Task is being DELAY! dt = [%f]", &example_dt);

        // rt_thread_delay(1);
}
